/* USER CODE BEGIN Header */
/**
  ******************************************************************************
  * @file           : main.c
  * @brief          : Main program body
  ******************************************************************************
  * @attention
  *
  * Copyright (c) 2025 STMicroelectronics.
  * @attention
  *
  * This software is licensed under terms that can be found in the LICENSE file
  * in the root directory of this software component.
  * If no LICENSE file comes with this software, it is provided AS-IS.
  *
  ******************************************************************************
  */
/* USER CODE END Header */
/* Includes ------------------------------------------------------------------*/
#include "main.h"
#include "dma.h"
#include "usart.h"
#include "gpio.h"

/* Private includes ----------------------------------------------------------*/
/* USER CODE BEGIN Includes */
#include "stdio.h"
#include "string.h"
#include "stdarg.h"
#include "stdlib.h"
#include "ctype.h"
/* USER CODE END Includes */

/* Private typedef -----------------------------------------------------------*/
/* USER CODE BEGIN PTD */
// 命令结构体
typedef struct {
    char cmd[20];
    uint16_t servo_id;
    uint16_t position;
    uint16_t time;
    uint16_t positions[ServoNum]; // 用于多舵机控制
    uint16_t move_time;           // 顺序控制的运动时间
} ServoCommand;
/* USER CODE END PTD */

/* Private define ------------------------------------------------------------*/
/* USER CODE BEGIN PD */


// 串口命令缓冲区
#define CMD_BUFFER_SIZE 128

// 调试输出宏
#define DEBUG_OUTPUT 1

#if DEBUG_OUTPUT
void debug_print(const char *format, ...);
void debug_output(const char *str, unsigned char *data, int len);
#else
#define debug_print(...)
#define debug_output(...)
#endif
/* USER CODE END PD */

/* Private macro -------------------------------------------------------------*/
/* USER CODE BEGIN PM */
void print_hex(unsigned char *data, int len);
/* USER CODE END PM */

/* Private variables ---------------------------------------------------------*/

/* USER CODE BEGIN PV */
uint8_t	ServoCmd[CmdMaxLength]={0x55,0x55,0x01,0x07,0x01,0x00,0x00,0xE8,0x03,0x0B};
uint8_t ServoRes[ResMaxLength]={0};

uint8_t UART5Rec_Flag=0;
static tsTimeType Timeout;

// 复位标志
uint8_t reset_flag = 1;

// 串口命令接收相关变量
uint8_t usart1_rx_buffer[CMD_BUFFER_SIZE];
uint8_t usart1_rx_index = 0;
uint8_t cmd_ready = 0;
/* USER CODE END PV */

/* Private function prototypes -----------------------------------------------*/
void SystemClock_Config(void);
/* USER CODE BEGIN PFP */
uint8_t CheckSum(uint8_t *Data, int len);
static void setTime(tsTimeType *TimeType, uint32_t TimeInter);
static uint8_t compareTime(tsTimeType *TimeType);
void move_servo_to_position(uint16_t servo_id, uint16_t position, uint16_t time_ms);
void move_all_servos_to_position(uint16_t position);
void move_multiple_servos(uint16_t* positions, uint16_t time_ms);
void sequential_move_servos(uint16_t* positions, uint16_t time_per_servo, uint16_t delay_between);
void wave_effect_forward(uint16_t start_pos, uint16_t end_pos, uint16_t time_per_servo, uint16_t delay_between);
void wave_effect_backward(uint16_t start_pos, uint16_t end_pos, uint16_t time_per_servo, uint16_t delay_between);
void read_servo_position(uint16_t servo_id);
void read_all_servos_position(void);
void process_serial_command(const char* command);
void parse_command(const char* cmd_str, ServoCommand* cmd);
void show_help(void);
void send_response(const char* response);
void test_servo_communication(void);
/* USER CODE END PFP */

/* Private user code ---------------------------------------------------------*/
/* USER CODE BEGIN 0 */

/* USER CODE END 0 */

/**
  * @brief  The application entry point.
  * @retval int
  */
int main(void)
{
  /* USER CODE BEGIN 1 */
	uint16_t Angle=0;
  /* USER CODE END 1 */

  /* MCU Configuration--------------------------------------------------------*/

  /* Reset of all peripherals, Initializes the Flash interface and the Systick. */
  HAL_Init();

  /* USER CODE BEGIN Init */

  /* USER CODE END Init */

  /* Configure the system clock */
  SystemClock_Config();

  /* USER CODE BEGIN SysInit */

  /* USER CODE END SysInit */

  /* Initialize all configured peripherals */
  MX_GPIO_Init();
  MX_DMA_Init();
  MX_UART5_Init();
  MX_USART1_UART_Init();
  /* USER CODE BEGIN 2 */
  __HAL_UART_CLEAR_IDLEFLAG(&huart5);                          // 清除空闲中断标志
  __HAL_UART_ENABLE_IT(&huart5, UART_IT_IDLE);                 // 使能空闲中断
  HAL_UART_Receive_DMA(&huart5, ServoRes, ResMaxLength);
  
  // 使能USART1接收中断
  HAL_UART_Receive_IT(&huart1, &usart1_rx_buffer[usart1_rx_index], 1);
  
  // 开机指示灯
  HAL_GPIO_WritePin(GPIOC, GPIO_PIN_7, GPIO_PIN_SET);
  HAL_Delay(500);
  HAL_GPIO_WritePin(GPIOC, GPIO_PIN_7, GPIO_PIN_RESET);
  
  debug_print("机械臂控制系统启动完成\r\n");
  debug_print("等待串口命令...\r\n");
  debug_print("输入 'help' 查看可用命令\r\n");
  debug_print("> ");
  /* USER CODE END 2 */

  /* Infinite loop */
  /* USER CODE BEGIN WHILE */
  while (1)
  {
    // 如果复位标志为1（刚复位），执行复位动作
    if(reset_flag)
    {
        debug_print("执行复位动作：所有舵机转到位置500\r\n");
        move_all_servos_to_position(500);
        reset_flag = 0;
        HAL_GPIO_WritePin(GPIOC, GPIO_PIN_7, GPIO_PIN_SET);
        HAL_Delay(1000);
        HAL_GPIO_WritePin(GPIOC, GPIO_PIN_7, GPIO_PIN_RESET);
        debug_print("复位动作完成\r\n");
        debug_print("> ");
    }
    
    // 检查是否有新的串口命令
    if(cmd_ready)
    {
        cmd_ready = 0;
        usart1_rx_buffer[usart1_rx_index] = '\0'; // 添加字符串结束符
        
        debug_print("收到命令: %s\r\n", usart1_rx_buffer);
        
        // 处理命令
        process_serial_command((char*)usart1_rx_buffer);
        
        // 重置接收缓冲区
        usart1_rx_index = 0;
        
        // 重新启动接收
        HAL_UART_Receive_IT(&huart1, &usart1_rx_buffer[usart1_rx_index], 1);
    }
    
    HAL_Delay(10);
    /* USER CODE END WHILE */

    /* USER CODE BEGIN 3 */
  }
  /* USER CODE END 3 */
}

/**
  * @brief System Clock Configuration
  * @retval None
  */
void SystemClock_Config(void)
{
  RCC_OscInitTypeDef RCC_OscInitStruct = {0};
  RCC_ClkInitTypeDef RCC_ClkInitStruct = {0};

  /** Configure the main internal regulator output voltage
  */
  __HAL_RCC_PWR_CLK_ENABLE();
  __HAL_PWR_VOLTAGESCALING_CONFIG(PWR_REGULATOR_VOLTAGE_SCALE1);

  /** Initializes the RCC Oscillators according to the specified parameters
  * in the RCC_OscInitTypeDef structure.
  */
  RCC_OscInitStruct.OscillatorType = RCC_OSCILLATORTYPE_HSI;
  RCC_OscInitStruct.HSIState = RCC_HSI_ON;
  RCC_OscInitStruct.HSICalibrationValue = RCC_HSICALIBRATION_DEFAULT;
  RCC_OscInitStruct.PLL.PLLState = RCC_PLL_ON;
  RCC_OscInitStruct.PLL.PLLSource = RCC_PLLSOURCE_HSI;
  RCC_OscInitStruct.PLL.PLLM = 8;
  RCC_OscInitStruct.PLL.PLLN = 168;
  RCC_OscInitStruct.PLL.PLLP = RCC_PLLP_DIV2;
  RCC_OscInitStruct.PLL.PLLQ = 4;
  if (HAL_RCC_OscConfig(&RCC_OscInitStruct) != HAL_OK)
  {
    Error_Handler();
  }

  /** Initializes the CPU, AHB and APB buses clocks
  */
  RCC_ClkInitStruct.ClockType = RCC_CLOCKTYPE_HCLK|RCC_CLOCKTYPE_SYSCLK
                              |RCC_CLOCKTYPE_PCLK1|RCC_CLOCKTYPE_PCLK2;
  RCC_ClkInitStruct.SYSCLKSource = RCC_SYSCLKSOURCE_PLLCLK;
  RCC_ClkInitStruct.AHBCLKDivider = RCC_SYSCLK_DIV1;
  RCC_ClkInitStruct.APB1CLKDivider = RCC_HCLK_DIV4;
  RCC_ClkInitStruct.APB2CLKDivider = RCC_HCLK_DIV2;

  if (HAL_RCC_ClockConfig(&RCC_ClkInitStruct, FLASH_LATENCY_5) != HAL_OK)
  {
    Error_Handler();
  }
}

/* USER CODE BEGIN 4 */
// USART1接收完成回调函数
void HAL_UART_RxCpltCallback(UART_HandleTypeDef *huart)
{
    if(huart->Instance == USART1)
    {
        // 检查是否是结束符（回车或换行）
        if(usart1_rx_buffer[usart1_rx_index] == '\r' || 
           usart1_rx_buffer[usart1_rx_index] == '\n')
        {
            if(usart1_rx_index > 0) // 确保不是空命令
            {
                cmd_ready = 1;
            }
        }
        else
        {
            // 继续接收下一个字符
            usart1_rx_index++;
            if(usart1_rx_index >= CMD_BUFFER_SIZE)
            {
                usart1_rx_index = 0; // 缓冲区溢出，重置
            }
            HAL_UART_Receive_IT(&huart1, &usart1_rx_buffer[usart1_rx_index], 1);
        }
    }
}

// 调试输出函数
#if DEBUG_OUTPUT
void debug_print(const char *format, ...)
{
    char buffer[128];
    va_list args;
    va_start(args, format);
    int len = vsnprintf(buffer, sizeof(buffer), format, args);
    va_end(args);
    
    if(len > 0 && HAL_UART_GetState(&huart1) == HAL_UART_STATE_READY)
    {
        HAL_UART_Transmit(&huart1, (uint8_t*)buffer, len, 50);
    }
}

void debug_output(const char *str, unsigned char *data, int len)
{
    HAL_UART_Transmit(&huart1, (uint8_t*)str, strlen(str), 10);
    for (int i = 0; i < len; i++)
    {
        char hex[4];
        snprintf(hex, sizeof(hex), "%02X ", data[i]);
        HAL_UART_Transmit(&huart1, (uint8_t*)hex, 3, 10);
    }
    HAL_UART_Transmit(&huart1, (uint8_t*)"\r\n", 2, 10);
}
#endif

#ifdef __GNUC__
#define PUTCHAR_PROTOTYPE int __io_putchar(int ch)

PUTCHAR_PROTOTYPE
{
    if(HAL_UART_GetState(&huart1) == HAL_UART_STATE_READY)
    {
        HAL_UART_Transmit(&huart1, (uint8_t *)&ch, 1, 50);
    }
    return ch;
}
#endif

/**
 * 函数功能： 设置软件定时器的定时值
 */
static void setTime(tsTimeType *TimeType, uint32_t TimeInter)
{
    TimeType->TimeStart = HAL_GetTick();
    TimeType->TimeInter = TimeInter;
}

/**
 * 函数功能： 比较软件定时器的值
 */
static uint8_t compareTime(tsTimeType *TimeType)
{
    return ((HAL_GetTick() - TimeType->TimeStart) >= TimeType->TimeInter);
}

/**
 * @brief 计算数据的校验和
 */
uint8_t CheckSum(uint8_t *Data, int len)
{
    uint8_t res = 0;
    for (int i = 0; i < len; i++)
    {
        res = (res + Data[i]) & 0xFF;
    }
    res = (~res) & 0xFF;
    return res;
}

void print_hex(unsigned char *data, int len)
{
    for (int i = 0; i < len; i++)
    {
        HAL_UART_Transmit(&huart5, &data[i], 1, 100);
    }
    HAL_UART_Transmit(&huart5, "\n", 1, 100);
}

/**
 * @brief 移动单个舵机到指定位置
 * @param servo_id 舵机ID (1-6)
 * @param position 目标位置 (0-1000)
 * @param time_ms 运动时间 (毫秒)
 */
void move_servo_to_position(uint16_t servo_id, uint16_t position, uint16_t time_ms)
{
    if(servo_id < 1 || servo_id > ServoNum)
    {
        debug_print("错误：舵机ID必须在1-%d之间\r\n", ServoNum);
        return;
    }
    
    if(position > 1000)
    {
        debug_print("警告：位置值超过1000，已限制为1000\r\n");
        position = 1000;
    }
    
    debug_print("移动舵机%d到位置%d，时间%dms\r\n", servo_id, position, time_ms);
    
    ServoCmd[2] = servo_id;
    ServoCmd[3] = SERVO_MOVE_TIME_WRITE_LEN;
    ServoCmd[4] = SERVO_MOVE_TIME_WRITE;
    ServoCmd[5] = position & 0xFF;
    ServoCmd[6] = (position >> 8) & 0xFF;
    ServoCmd[7] = time_ms & 0xFF;
    ServoCmd[8] = (time_ms >> 8) & 0xFF;
    ServoCmd[9] = CheckSum(&ServoCmd[2], SERVO_MOVE_TIME_WRITE_LEN);
    
    // 设置发送模式
    HAL_GPIO_WritePin(UART5_RX_EN_GPIO_Port, UART5_RX_EN_Pin, GPIO_PIN_RESET);
    HAL_GPIO_WritePin(UART5_TX_EN_GPIO_Port, UART5_TX_EN_Pin, GPIO_PIN_SET);
    HAL_Delay(1); // 确保电平稳定
    
    // 发送指令
    HAL_UART_Transmit(&huart5, ServoCmd, 10, 100);
    
    // 立即切换回接收模式
    HAL_GPIO_WritePin(UART5_TX_EN_GPIO_Port, UART5_TX_EN_Pin, GPIO_PIN_RESET);
    HAL_GPIO_WritePin(UART5_RX_EN_GPIO_Port, UART5_RX_EN_Pin, GPIO_PIN_SET);
    HAL_Delay(1);
    
    debug_output("发送指令: ", ServoCmd, 10);
    
    // 等待运动完成
    HAL_Delay(time_ms + 200);
}

/**
 * @brief 移动所有舵机到指定位置
 */
void move_all_servos_to_position(uint16_t position)
{
    debug_print("移动所有舵机到位置: %d\r\n", position);
    
    for(int i = 1; i <= ServoNum; i++)
    {
        move_servo_to_position(i, position, 1000);
        HAL_Delay(50);
    }
    
    debug_print("所有舵机移动完成\r\n");
}

/**
 * @brief 同时移动多个舵机到不同位置
 * @param positions 位置数组，positions[0]对应舵机1，以此类推
 * @param time_ms 运动时间 (毫秒)
 */
void move_multiple_servos(uint16_t* positions, uint16_t time_ms)
{
    debug_print("同时移动多个舵机: ");
    for(int i = 0; i < ServoNum; i++)
    {
        if(positions[i] != 0xFFFF) // 0xFFFF表示不移动该舵机
        {
            debug_print("舵机%d->%d ", i+1, positions[i]);
        }
    }
    debug_print("\r\n");
    
    // 先发送所有指令
    for(int i = 0; i < ServoNum; i++)
    {
        if(positions[i] != 0xFFFF) // 只移动指定位置不为0xFFFF的舵机
        {
            uint16_t pos = positions[i];
            if(pos > 1000) pos = 1000;
            
            ServoCmd[2] = i + 1; // 舵机ID从1开始
            ServoCmd[3] = SERVO_MOVE_TIME_WRITE_LEN;
            ServoCmd[4] = SERVO_MOVE_TIME_WRITE;
            ServoCmd[5] = pos & 0xFF;
            ServoCmd[6] = (pos >> 8) & 0xFF;
            ServoCmd[7] = time_ms & 0xFF;
            ServoCmd[8] = (time_ms >> 8) & 0xFF;
            ServoCmd[9] = CheckSum(&ServoCmd[2], SERVO_MOVE_TIME_WRITE_LEN);
            
            // 设置发送模式
            HAL_GPIO_WritePin(UART5_RX_EN_GPIO_Port, UART5_RX_EN_Pin, GPIO_PIN_RESET);
            HAL_GPIO_WritePin(UART5_TX_EN_GPIO_Port, UART5_TX_EN_Pin, GPIO_PIN_SET);
            HAL_Delay(1);
            
            // 发送指令
            HAL_UART_Transmit(&huart5, ServoCmd, 10, 100);
            
            debug_output("发送指令: ", ServoCmd, 10);
        }
    }
    
    // 切换回接收模式
    HAL_GPIO_WritePin(UART5_TX_EN_GPIO_Port, UART5_TX_EN_Pin, GPIO_PIN_RESET);
    HAL_GPIO_WritePin(UART5_RX_EN_GPIO_Port, UART5_RX_EN_Pin, GPIO_PIN_SET);
    HAL_Delay(1);
    
    // 等待运动完成
    HAL_Delay(time_ms + 200);
    debug_print("多舵机移动完成\r\n");
}

/**
 * @brief 顺序移动多个舵机到不同位置（从第一个舵机开始依次执行）
 * @param positions 位置数组，positions[0]对应舵机1，以此类推
 * @param time_per_servo 每个舵机的运动时间
 * @param delay_between 舵机之间的延迟时间
 */
void sequential_move_servos(uint16_t* positions, uint16_t time_per_servo, uint16_t delay_between)
{
    debug_print("顺序移动舵机: ");
    for(int i = 0; i < ServoNum; i++)
    {
        if(positions[i] != 0xFFFF) // 0xFFFF表示不移动该舵机
        {
            debug_print("舵机%d->%d ", i+1, positions[i]);
        }
    }
    debug_print("\r\n");
    debug_print("运动时间:%dms, 间隔:%dms\r\n", time_per_servo, delay_between);
    
    // 从第一个舵机开始依次执行
    for(int i = 0; i < ServoNum; i++)
    {
        if(positions[i] != 0xFFFF) // 只移动指定位置不为0xFFFF的舵机
        {
            uint16_t pos = positions[i];
            if(pos > 1000) pos = 1000;
            
            debug_print("执行: 舵机%d -> 位置%d\r\n", i+1, pos);
            move_servo_to_position(i+1, pos, time_per_servo);
            
            // 等待间隔时间（如果还有下一个舵机要移动）
            if(i < ServoNum - 1 && positions[i+1] != 0xFFFF)
            {
                HAL_Delay(delay_between);
            }
        }
    }
    
    debug_print("顺序移动完成\r\n");
}

/**
 * @brief 从前往后波浪效果（0号舵机开始）
 * @param start_pos 起始位置
 * @param end_pos 结束位置
 * @param time_per_servo 每个舵机运动时间
 * @param delay_between 舵机之间的延迟
 */
void wave_effect_forward(uint16_t start_pos, uint16_t end_pos, uint16_t time_per_servo, uint16_t delay_between)
{
    debug_print("开始从前往后波浪效果: %d -> %d, 时间:%dms, 间隔:%dms\r\n", 
                start_pos, end_pos, time_per_servo, delay_between);
    
    // 所有舵机先回到起始位置
    for(int i = 1; i <= ServoNum; i++)
    {
        move_servo_to_position(i, start_pos, time_per_servo);
    }
    HAL_Delay(time_per_servo + 200);
    
    // 从0号舵机开始依次运动到结束位置
    for(int i = 1; i <= ServoNum; i++)
    {
        debug_print("波浪前进: 舵机%d运动到%d\r\n", i, end_pos);
        move_servo_to_position(i, end_pos, time_per_servo);
        HAL_Delay(delay_between);
    }
    
    debug_print("从前往后波浪效果完成\r\n");
}

/**
 * @brief 从后往前波浪效果（最后一个舵机开始）
 * @param start_pos 起始位置
 * @param end_pos 结束位置
 * @param time_per_servo 每个舵机运动时间
 * @param delay_between 舵机之间的延迟
 */
void wave_effect_backward(uint16_t start_pos, uint16_t end_pos, uint16_t time_per_servo, uint16_t delay_between)
{
    debug_print("开始从后往前波浪效果: %d -> %d, 时间:%dms, 间隔:%dms\r\n", 
                start_pos, end_pos, time_per_servo, delay_between);
    
    // 所有舵机先回到起始位置
    for(int i = 1; i <= ServoNum; i++)
    {
        move_servo_to_position(i, start_pos, time_per_servo);
    }
    HAL_Delay(time_per_servo + 200);
    
    // 从最后一个舵机开始依次运动到结束位置
    for(int i = ServoNum; i >= 1; i--)
    {
        debug_print("波浪后退: 舵机%d运动到%d\r\n", i, end_pos);
        move_servo_to_position(i, end_pos, time_per_servo);
        HAL_Delay(delay_between);
    }
    
    debug_print("从后往前波浪效果完成\r\n");
}

/**
 * @brief 读取单个舵机位置
 */
void read_servo_position(uint16_t servo_id)
{
    if(servo_id < 1 || servo_id > ServoNum)
    {
        debug_print("错误：舵机ID必须在1-%d之间\r\n", ServoNum);
        return;
    }
    
    ServoCmd[2] = servo_id;
    ServoCmd[3] = SERVO_POS_READ_LEN;
    ServoCmd[4] = SERVO_POS_READ;
    ServoCmd[5] = CheckSum(&ServoCmd[2], SERVO_POS_READ_LEN);
    
    UART5Rec_Flag = 0;
    HAL_GPIO_WritePin(UART5_RX_EN_GPIO_Port, UART5_RX_EN_Pin, GPIO_PIN_RESET);
    HAL_GPIO_WritePin(UART5_TX_EN_GPIO_Port, UART5_TX_EN_Pin, GPIO_PIN_SET);
    HAL_UART_Transmit(&huart5, ServoCmd, 6, 100);
    HAL_GPIO_WritePin(UART5_RX_EN_GPIO_Port, UART5_RX_EN_Pin, GPIO_PIN_SET);
    HAL_GPIO_WritePin(UART5_TX_EN_GPIO_Port, UART5_TX_EN_Pin, GPIO_PIN_RESET);
    
    debug_output("发送读取指令: ", ServoCmd, 6);
    
    setTime(&Timeout, 500);
    while(UART5Rec_Flag == 0)
    {
        if (compareTime(&Timeout))
        {
            debug_print("舵机 %d 读取超时\r\n", servo_id);
            return;
        }
    }
    
    if(UART5Rec_Flag)
    {
        uint16_t current_angle = ServoRes[5] | (ServoRes[6] << 8);
        debug_print("舵机 %d 当前位置: %d\r\n", servo_id, current_angle);
    }
}

/**
 * @brief 读取所有舵机位置
 */
void read_all_servos_position(void)
{
    debug_print("读取所有舵机位置...\r\n");
    
    for(int i = 1; i <= ServoNum; i++)
    {
        read_servo_position(i);
        HAL_Delay(100);
    }
}

/**
 * @brief 显示帮助信息
 */
void show_help(void)
{
    debug_print("=== 机械臂控制命令 ===\r\n");
    debug_print("move <id> <pos> [time] - 移动单个舵机\r\n");
    debug_print("                        id: 舵机ID(1-%d), pos: 位置(0-1000), time: 时间(ms)\r\n", ServoNum);
    debug_print("moveall <pos>          - 移动所有舵机到相同位置\r\n");
    debug_print("movemulti <pos1,pos2,...> - 同时移动多个舵机到不同位置\r\n");
    debug_print("                          例: movemulti 500,300,700 (移动1-3号舵机)\r\n");
    debug_print("                          例: movemulti 500,-,300,-,700 (只移动1,3,5号舵机)\r\n");
    debug_print("seqmove <pos1,pos2,...> [time] [delay] - 顺序移动多个舵机\r\n");
    debug_print("                          例: seqmove 500,300,700 (1->500, 2->300, 3->700)\r\n");
    debug_print("                          例: seqmove 500,-,300,-,700 800 100 (时间800ms, 间隔100ms)\r\n");
    debug_print("wavef <start> <end> [time] [delay] - 从前往后波浪效果\r\n");
    debug_print("waveb <start> <end> [time] [delay] - 从后往前波浪效果\r\n");
    debug_print("read <id>              - 读取单个舵机位置\r\n");
    debug_print("readall                - 读取所有舵机位置\r\n");
    debug_print("reset                  - 复位所有舵机到500位置\r\n");
    debug_print("test                   - 测试舵机通信\r\n");
    debug_print("help                   - 显示此帮助信息\r\n");
    debug_print("status                 - 显示系统状态\r\n");
    debug_print("========================\r\n");
}

/**
 * @brief 解析命令字符串
 */
void parse_command(const char* cmd_str, ServoCommand* cmd)
{
    char temp[128];
    strncpy(temp, cmd_str, sizeof(temp)-1);
    temp[sizeof(temp)-1] = '\0';
    
    // 转换为小写
    for(int i = 0; temp[i]; i++)
    {
        temp[i] = tolower(temp[i]);
    }
    
    // 初始化默认值
    memset(cmd, 0, sizeof(ServoCommand));
    cmd->time = 1000; // 默认1秒
    cmd->move_time = 1000; // 默认顺序移动时间
    
    // 初始化多舵机位置数组
    for(int i = 0; i < ServoNum; i++)
    {
        cmd->positions[i] = 0xFFFF; // 默认不移动
    }
    
    // 命令解析
    if(strstr(temp, "seqmove"))
    {
        strcpy(cmd->cmd, "seqmove");
        
        // 解析多舵机位置，格式: seqmove 500,300,700 或 seqmove 500,-,300,-,700 [time] [delay]
        char* token = strtok(temp + 8, " ,"); // 使用空格或逗号分隔
        
        int index = 0;
        int param_count = 0;
        uint16_t params[2] = {1000, 200}; // 默认时间1000ms，间隔200ms
        
        while(token != NULL)
        {
            // 跳过空格
            while(*token == ' ') token++;
            
            if(index < ServoNum)
            {
                if(strcmp(token, "-") == 0)
                {
                    // 破折号表示跳过该舵机
                    cmd->positions[index] = 0xFFFF;
                    index++;
                }
                else if(isdigit(*token))
                {
                    // 解析位置值
                    cmd->positions[index] = atoi(token);
                    index++;
                }
            }
            else
            {
                // 解析时间和延迟参数
                if(param_count < 2)
                {
                    params[param_count] = atoi(token);
                    param_count++;
                }
            }
            
            token = strtok(NULL, " ,");
        }
        
        cmd->move_time = params[0];
        cmd->time = params[1]; // 重用time字段作为延迟时间
    }
    else if(strstr(temp, "wavef"))
    {
        strcpy(cmd->cmd, "wavef");
        // 解析参数: wavef start_pos end_pos [time] [delay]
        sscanf(temp, "wavef %hu %hu %hu %hu", 
               &cmd->positions[0], &cmd->positions[1], &cmd->time, &cmd->servo_id);
        // 如果没有提供时间和延迟，使用默认值
        if(cmd->time == 0) cmd->time = 1000;
        if(cmd->servo_id == 0) cmd->servo_id = 200; // 默认延迟200ms
    }
    else if(strstr(temp, "waveb"))
    {
        strcpy(cmd->cmd, "waveb");
        // 解析参数: waveb start_pos end_pos [time] [delay]
        sscanf(temp, "waveb %hu %hu %hu %hu", 
               &cmd->positions[0], &cmd->positions[1], &cmd->time, &cmd->servo_id);
        // 如果没有提供时间和延迟，使用默认值
        if(cmd->time == 0) cmd->time = 1000;
        if(cmd->servo_id == 0) cmd->servo_id = 200; // 默认延迟200ms
    }
    else if(strstr(temp, "movemulti"))
    {
        strcpy(cmd->cmd, "movemulti");
        
        // 解析多舵机位置，格式: movemulti 500,300,700 或 movemulti 500,-,300,-,700
        char* token = strtok(temp + 10, ",");
        int index = 0;
        
        while(token != NULL && index < ServoNum)
        {
            // 跳过空格
            while(*token == ' ') token++;
            
            if(strcmp(token, "-") == 0)
            {
                // 破折号表示跳过该舵机
                cmd->positions[index] = 0xFFFF;
            }
            else
            {
                // 解析位置值
                cmd->positions[index] = atoi(token);
            }
            
            token = strtok(NULL, ",");
            index++;
        }
    }
    else if(strstr(temp, "moveall"))
    {
        strcpy(cmd->cmd, "moveall");
        sscanf(temp, "moveall %hu", &cmd->position);
    }
    else if(strstr(temp, "move"))
    {
        strcpy(cmd->cmd, "move");
        sscanf(temp, "move %hu %hu %hu", &cmd->servo_id, &cmd->position, &cmd->time);
        if(cmd->time == 0) cmd->time = 1000;
    }
    else if(strstr(temp, "readall"))
    {
        strcpy(cmd->cmd, "readall");
    }
    else if(strstr(temp, "read"))
    {
        strcpy(cmd->cmd, "read");
        sscanf(temp, "read %hu", &cmd->servo_id);
    }
    else if(strstr(temp, "reset"))
    {
        strcpy(cmd->cmd, "reset");
    }
    else if(strstr(temp, "help"))
    {
        strcpy(cmd->cmd, "help");
    }
    else if(strstr(temp, "status"))
    {
        strcpy(cmd->cmd, "status");
    }
    else if(strstr(temp, "test"))
    {
        strcpy(cmd->cmd, "test");
    }
    else
    {
        strcpy(cmd->cmd, "unknown");
    }
}

/**
 * @brief 处理串口命令
 */
void process_serial_command(const char* command)
{
    ServoCommand cmd;
    parse_command(command, &cmd);
    
    if(strcmp(cmd.cmd, "move") == 0)
    {
        move_servo_to_position(cmd.servo_id, cmd.position, cmd.time);
        debug_print("命令执行完成\r\n");
    }
    else if(strcmp(cmd.cmd, "moveall") == 0)
    {
        move_all_servos_to_position(cmd.position);
        debug_print("命令执行完成\r\n");
    }
    else if(strcmp(cmd.cmd, "movemulti") == 0)
    {
        move_multiple_servos(cmd.positions, 1000);
        debug_print("多舵机控制完成\r\n");
    }
    else if(strcmp(cmd.cmd, "seqmove") == 0)  // 新增顺序移动命令
    {
        sequential_move_servos(cmd.positions, cmd.move_time, cmd.time);
        debug_print("顺序移动完成\r\n");
    }
    else if(strcmp(cmd.cmd, "wavef") == 0)
    {
        wave_effect_forward(cmd.positions[0], cmd.positions[1], cmd.time, cmd.servo_id);
        debug_print("从前往后波浪效果完成\r\n");
    }
    else if(strcmp(cmd.cmd, "waveb") == 0)
    {
        wave_effect_backward(cmd.positions[0], cmd.positions[1], cmd.time, cmd.servo_id);
        debug_print("从后往前波浪效果完成\r\n");
    }
    else if(strcmp(cmd.cmd, "read") == 0)
    {
        read_servo_position(cmd.servo_id);
    }
    else if(strcmp(cmd.cmd, "readall") == 0)
    {
        read_all_servos_position();
    }
    else if(strcmp(cmd.cmd, "reset") == 0)
    {
        move_all_servos_to_position(500);
        debug_print("复位完成\r\n");
    }
    else if(strcmp(cmd.cmd, "help") == 0)
    {
        show_help();
    }
    else if(strcmp(cmd.cmd, "status") == 0)
    {
        debug_print("系统状态: 运行正常\r\n");
        debug_print("舵机数量: %d\r\n", ServoNum);
        debug_print("输入 'help' 查看可用命令\r\n");
    }
    else if(strcmp(cmd.cmd, "test") == 0)
    {
        test_servo_communication();
    }
    else
    {
        debug_print("未知命令: %s\r\n", command);
        debug_print("输入 'help' 查看可用命令\r\n");
    }
    
    debug_print("> "); // 命令提示符
}

/**
 * @brief 测试舵机通信
 */
void test_servo_communication(void)
{
    debug_print("开始舵机通信测试...\r\n");
    
    // 测试顺序控制
    debug_print("测试顺序控制功能...\r\n");
    uint16_t test_positions[ServoNum] = {100, 300, 500, 700, 900, 200};
    sequential_move_servos(test_positions, 800, 300);
    HAL_Delay(1000);
    
    // 回到中间位置
    move_all_servos_to_position(500);
    
    debug_print("舵机通信测试完成\r\n");
}
/* USER CODE END 4 */

/**
  * @brief  This function is executed in case of error occurrence.
  * @retval None
  */
void Error_Handler(void)
{
  /* USER CODE BEGIN Error_Handler_Debug */
  __disable_irq();
  while (1)
  {
    HAL_GPIO_TogglePin(GPIOC, GPIO_PIN_7);
    HAL_Delay(200);
  }
  /* USER CODE END Error_Handler_Debug */
}

#ifdef  USE_FULL_ASSERT
/**
  * @brief  Reports the name of the source file and the source line number
  *         where the assert_param error has occurred.
  * @param  file: pointer to the source file name
  * @param  line: assert_param error line source number
  * @retval None
  */
void assert_failed(uint8_t *file, uint32_t line)
{
  /* USER CODE BEGIN 6 */
  /* USER CODE END 6 */
}
#endif /* USE_FULL_ASSERT */